
#ifndef DeltaKinematicsKe_h
#define DeltaKinematicsKe_h

#define sqrt3 1.7320508075688772
#define pi  3.141592653     // PI
#define sin120  sqrt3/2.0   
#define cos120  -0.5        
#define tan60  sqrt3
#define sin30  0.5
#define tan30  1.0/sqrt3

#define non_existing_povar_error -2
#define no_error 1

class DeltaKinematicsKe
{
    public:
        // SETUP 
        DeltaKinematicsKe(double _ArmLength_short, double armBiasIn,
                          double _RodLength, double topCircleIn, double buttomCircleIn,
                          double zBiasIn);
        

        int forward_bias(double thetaA, double thetaB, double thetaC,
                    double &xOut, double &yOut, double &zOut);

        int inverse_bias(double x0, double y0, double z0,double &aOut,
                         double &bOut, double &cOut);

        int forward(double thetaA, double thetaB, double thetaC,
                    double &xOut, double &yOut, double &zOut);

        int inverse(double x0, double y0, double z0,double &aOut, double &bOut, double &cOut);


        
    private:
    
        int delta_calcAngleYZ(double *Angle, double x0, double y0, double z0);

        double ArmLength;//短臂
        double RodLength;//长臂
        double topTriangleSideLength;//顶部三角形边长
        double buttomTriangleSideLength;//底部三角形边长
        double _zPositionBias;//法兰盘Z偏移　m
        double _zeroBiasAngle;//关节零点偏移　degree
       
};

#endif 



